import machine
import time

# Setup I2C communication (GPIO 4 for SDA, GPIO 5 for SCL on Raspberry Pi Pico)
i2c = machine.I2C(0, sda=machine.Pin(4), scl=machine.Pin(5))  # Adjust GPIO pins as needed

# VEML6040 I2C address and registers
VEML6040_ADDR = 0x10  # Default I2C address of the VEML6040 (check datasheet for your sensor's address)
REG_CONFIG = 0x00  # Configuration register
REG_RED = 0x08     # Register for Red data
REG_GREEN = 0x09   # Register for Green data
REG_BLUE = 0x0A    # Register for Blue data
REG_CLEAR = 0x0B   # Register for Clear data

# Function to write to a register using `writeto_mem`
def write_register(reg, value):
    try:
        i2c.writeto_mem(VEML6040_ADDR, reg, bytearray([value]))
    except Exception as e:
        print(f"Failed to write to register {reg}: {e}")

# Function to read 16-bit data from a register using `readfrom_mem`
def read_register(reg):
    try:
        data = i2c.readfrom_mem(VEML6040_ADDR, reg, 2)  # Read 2 bytes (16-bit data)
        return int.from_bytes(data, 'little')  # Convert to integer (little-endian)
    except Exception as e:
        print(f"Failed to read from register {reg}: {e}")
        return 0

# Function to initialize the VEML6040 sensor
def init_sensor():
    try:
        # Example configuration: Enable the sensor with default settings
        config_value = 0x00  # Adjust based on the VEML6040 datasheet
        write_register(REG_CONFIG, config_value)
        print("Sensor initialized successfully.")
        time.sleep(0.1)  # Allow time for the sensor to initialize
    except Exception as e:
        print(f"Failed to initialize sensor: {e}")

# Function to read RGB values from the sensor
def read_raw_rgb():
    try:
        red = read_register(REG_RED)
        green = read_register(REG_GREEN)
        blue = read_register(REG_BLUE)
        return red, green, blue
    except Exception as e:
        print(f"Error reading raw RGB data: {e}")
        return 0, 0, 0

# Function to calibrate using standard RGB and black/white values
def calibrate():
    print("Calibration process started. Please follow the instructions.")

    # Read and store standard red
    input("Place the sensor in front of standard red (220, 0, 0) and press Enter.")
    red_std, _, _ = read_raw_rgb()
    print(f"Standard red value read: {red_std}")

    # Read and store standard green
    input("Place the sensor in front of standard green (0, 220, 0) and press Enter.")
    _, green_std, _ = read_raw_rgb()
    print(f"Standard green value read: {green_std}")

    # Read and store standard blue
    input("Place the sensor in front of standard blue (0, 0, 220) and press Enter.")
    _, _, blue_std = read_raw_rgb()
    print(f"Standard blue value read: {blue_std}")

    # Read and store standard white
    input("Place the sensor in front of standard white and press Enter.")
    white_red, white_green, white_blue = read_raw_rgb()
    print(f"Standard white values read: Red={white_red}, Green={white_green}, Blue={white_blue}")

    # Read and store standard black
    input("Place the sensor in front of standard black and press Enter.")
    black_red, black_green, black_blue = read_raw_rgb()
    print(f"Standard black values read: Red={black_red}, Green={black_green}, Blue={black_blue}")

    print("Calibration complete.")
    return (red_std, green_std, blue_std, 
            white_red, white_green, white_blue, 
            black_red, black_green, black_blue)

# Fine-tuned function to map raw values to normalized RGB
def map_to_rgb(raw_value, black_value, white_value):
    # Subtract black and cap to white
    normalized_value = max(0, raw_value - black_value)  # Ensure non-negative
    scaled_value = (normalized_value / (white_value - black_value)) * 255 if (white_value - black_value) > 0 else 0
    return min(255, int(scaled_value))  # Cap to 255

# Function to read and print normalized RGB data
def read_rgb(calibration_data):
    try:
        red, green, blue = read_raw_rgb()

        (red_std, green_std, blue_std, 
         white_red, white_green, white_blue, 
         black_red, black_green, black_blue) = calibration_data

        # Normalize values using calibration data
        red_normalized = map_to_rgb(red, black_red, white_red)
        green_normalized = map_to_rgb(green, black_green, white_green)
        blue_normalized = map_to_rgb(blue, black_blue, white_blue)

        print(f"Raw Values: Red={red}, Green={green}, Blue={blue}")
        print(f"Normalized RGB: R={red_normalized}, G={green_normalized}, B={blue_normalized}")
    except Exception as e:
        print(f"Error reading RGB data: {e}")

# Initialize the sensor
init_sensor()

# Calibration phase
calibration_data = calibrate()

# Main loop to continuously read RGB values
while True:
    read_rgb(calibration_data)
    time.sleep(3)  # Read values every 3 seconds